#pragma once

#include <mutex>
#include <thread>
#include <vector>

#include "common.hpp"
#include "rplidar.h"
#include "semaphore.hpp"
#include "sl_lidar.h"
#include "usb.hpp"

class Lidar {
 private:
  USB usb_;
  sl::ILidarDriver* driver_;
  std::vector<std::vector<component::LidarPoint>> datas_;
  bool scanning_;

  std::thread thread_lidar_;
  std::mutex mutex_;
  component::Semaphore signal_;

  bool Init();
  void ThreadScan();

 public:
  Lidar();
  ~Lidar();

  bool Open();
  bool Disconnect();
  // bool GetData(std::vector<component::LidarPoint>& data);
  std::vector<component::LidarPoint> GetData();
  bool Stop();
};

class LidarDriver {
 private:
  Lidar lidar_;
  std::vector<component::LidarPoint> inners_;
  std::vector<float> measured_;
  // float measured_[720];
  double min_, max_;

  bool CalObstrcts(std::vector<component::LidarPoint> points);

 public:
  LidarDriver();
  LidarDriver(double a, double b);
  ~LidarDriver();

  bool Init();
  void SetLimit(double a, double b);
  std::vector<float> Apply();
  std::vector<component::LidarPoint> GetInner() { return inners_; }
  std::vector<uint8_t> GetResult();
};